#include "CHaptikDevice.h"
#include "CHaptik3dofPointer.h"
#include "CPrecisionTimer.h"
#include "CWorld.h"

#include <windows.h>
#include <conio.h>
#include <stdio.h>

//Haptic timer
cPrecisionTimer timer;

//Virtual device
cHaptik3dofPointer* tool = 0;
//Virtual world
cWorld* world;

//Haptic timer callback
void haptic_loop(void * param) {

	//Read the position of the haptic device and
	//update the position of the device in the workspace
    tool->updatePose();

	//If button 0 pressed print the position of the device in the "virtual workspace"
	if(tool->buttonStateZero() == 1){
	printf("\nPOSITION IN THE VIRTUAL WORLD\n x: [%0.3f] y: [%0.3f] z: [%0.3f]\n",
		tool->m_deviceGlobalPos.x,tool->m_deviceGlobalPos.y,tool->m_deviceGlobalPos.z);
	}

	cVector3d force;
	force.zero();
	//Apply a simple force-feedback
    force.x = -(tool->m_deviceGlobalPos.x)*tool->m_workspaceScale.x/tool->m_halfWorkspaceAxisX;
	force.y = -(tool->m_deviceGlobalPos.y)*tool->m_workspaceScale.y/tool->m_halfWorkspaceAxisY;
	force.z = -(tool->m_deviceGlobalPos.z)*tool->m_workspaceScale.z/tool->m_halfWorkspaceAxisZ;

	//Update result
    tool->m_lastComputedGlobalForce.copyfrom(force);

	tool->applyForces();

}

void main(){

	int result = 0;

	world = new cWorld();
	tool = new cHaptik3dofPointer(world);

	result = tool->startHaptikLibrary();

	if FAIL(result){
		printf("%s",tool->getResultToString(result));
		delete tool;
		delete world;
	    getch();
	    return;
	}

	//Print the list of connected devices
	printf("\nConnected devices:\n");

	for(UINT32 i = 0 ; i < tool->getNumberOfDevices() ; i++){

		printf("\n[%2d] NAME         : %s\n     MODEL        : %s\n     MANUFACTURER : %s\n",
			i,tool->readDeviceInfoFromLibrary(i,DEVICE_NAME),
			  tool->readDeviceInfoFromLibrary(i,DEVICE_MODEL),
			  tool->readDeviceInfoFromLibrary(i,DEVICE_MANUFACTURER));
    }

    UINT32 choice = -1;
    char8 buffer[10];
	printf("\n CHOOSE A DEVICE FROM THE DEVICE LIST: \n\n");
    sscanf( gets(buffer) , "%u", &choice);

	result = tool->getHaptikDevice(choice);

	if FAIL(result){
		printf("%s",tool->getResultToString(result));
		delete tool;
		delete world;
	    getch();
	    return;
	}

	if ADVICE(result)
	printf("%s",tool->getResultToString(result));

	world->addChild(tool);

	//Set only basic characteristics of the tool
	tool->setPos(0.0, 0.0, 0.0);
    tool->setWorkspace(3.0,3.0,3.0);

	printf("\nVIRUTUAL WORKSPACE SET: x: [3.0] y: [3.0] z: [3.0]\n");

	tool->initialize();

	//Open communication with the device
	result = tool->start();

	if ADVICE(result)
	printf("%s",tool->getResultToString(result));

	if FAIL(result){
		printf("%s",tool->getResultToString(result));
	    world->removeChild(tool);
		tool->releaseInterface();
		delete tool;
		delete world;
	    getch();
	    return;
	}

	//Start haptic timer callback
	timer.set(100, haptic_loop, NULL);

	Sleep(100);

	getch();

	//Stop timer, device communication and release interface
	timer.stop();
	tool->stop();
	tool->releaseInterface();
	world->removeChild(tool);

	delete tool;
	delete world;

}